/*
 * pid.c
 *
 *  Created on: Sep 19, 2025
 *      Author: lianz
 */

#include "pid.h"
#include "stm32f1xx_hal.h"
#include "tim.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "mpu6050.h"
#include "encoder.h"
#include "motor.h"
#include "OLED.h"
#include "SR04.h"
#include "remote.h"

// 前进速度限幅
#define V_MAX	80
#define V_MIN	0


// pid系数
float Vertical_Kp = 570 * 0.6, Vertical_Kd = 3.1 * 0.6;	// 直立环  0 ~ 1000  0 ~ 10
float Velocity_Kp = -0.4, Velocity_Ki = 0;	// 速度环  0 ~ 1
float Turn_Kp = 10, Turn_Kd = 0.8;			// 转向环
// pid目标
float med_angle = 0;					// 直立环
int speed_target = 0;				// 速度环
int target_turn = 0;				// 转向环
// 控制增量
int stright_plus = 0, turn_plus = 0, forward_plus = 0, back_plus = 0, left_plus = 0, right_plus = 0;
// 控制变量
int pwm_out, velocity_out, vertical_out, turn_out, total_out, moto1, moto2;
// 读取数据
float pitch, roll, yaw;
short gyrox, gyroy, gyroz, aacx, aacy, aacz;
int Lencoder, Rencoder;
// 状态机
uint8_t control_cmd;
uint8_t angle_stop_cmd;
uint8_t force_stop_cmd;

// 直立环控制器
// 输入：期望角度， 真是角度， 角速度
int Vertical(float Med, float Angle, float gyro_Y)
{
	int res;
	// 计算输出值
	res = Vertical_Kp * (Med - Angle) - Vertical_Kd * gyro_Y;
	return res;
}

// 速度环控制器
// 输入：期望速度， 左编码器， 右编码器
int Velocity(float Target, int Lencoder, int Rencoder)
{
	static float a = 0.8;
	int Err, Err_LowOut, res;
	static int integral, Err_LowOut_Last;

	Velocity_Ki = Velocity_Kp / 200;

	// 计算偏差
	Err = (Target - Lencoder - Rencoder);
	// 低通滤波
	Err_LowOut = Err_LowOut_Last * a + Err * (1 - a);
	Err_LowOut_Last = Err_LowOut;
	// 积分值
	integral += Err_LowOut;
	// 积分限幅
	integral = integral > 20000 ? 20000 : integral;
	integral = integral < -20000 ? -20000 : integral;
	// ------------ 智能感应 ------------
	// 前进前进或者后退时，速度积分清零
	if (forward == 1 || back == 1) integral = 0;
	// 角度感应锁锁住时，速度积分清零
	if (angle_stop_cmd == 1) integral = 0;
	// -------------------------------
	// 速度环输出值
	res = Velocity_Kp * Err_LowOut + Velocity_Ki * integral;
	// 输出
	return res;
}

// 转向环控制
// 输入：角速度， 角度值
int Turn(float gyro_Z, int Target_Turn)
{
	int res;
	res = Turn_Kp * Target_Turn - Turn_Kd * gyro_Z;		// 此处把每次的真实角度值当0，以实现持续偏转而不是固定偏转角度
	return res;
}

// 速度方向控制
void Plus(void)
{
	// 遥控
	if (forward == 1)
	{
		// 速度递增
		forward_plus++;
		// 最小值赋值
		forward_plus = (forward_plus <= V_MIN) ? V_MIN : forward_plus;
		// 最大值限幅
		forward_plus = (forward_plus >= V_MAX) ? V_MAX : forward_plus;
	}
	else forward_plus = 0;

	if (back == 1)
	{
		// 速度递增
		back_plus--;
		// 最小值赋值
		back_plus = (back_plus >= -V_MIN) ? -V_MIN : back_plus;
		// 最大值限幅
		back_plus = (back_plus <= -V_MAX) ? -V_MAX : back_plus;
	}
	else back_plus = 0;

	if (left == 1) left_plus = 1200;
	else left_plus = 0;
	if (right == 1) right_plus = -1200;
	else right_plus = 0;
	stright_plus = forward_plus + back_plus;
	turn_plus = left_plus + right_plus;
}

void Control(void)
{
	if (control_cmd != 1) return;
	// 读取编码器，加速度，速度
	Lencoder = Read_Speed(&htim2);
	Rencoder = -Read_Speed(&htim4);
	mpu_dmp_get_data(&pitch, &roll, &yaw);
	MPU_Get_Gyroscope(&gyrox, &gyroy, &gyroz);
	MPU_Get_Accelerometer(&aacx, &aacy, &aacz);

	// 方向速度控制
	Plus();
	// 速度限幅
	stright_plus = stright_plus > 80 ? 80 : stright_plus;
	stright_plus = stright_plus < -80 ? -80 :stright_plus;
	// pid控制器
	velocity_out = Velocity(stright_plus, Lencoder, Rencoder);	//目标角度为角度中值加角度附加值
	vertical_out = Vertical(velocity_out + med_angle, roll, gyrox);
	//偏航回正补偿
	turn_out = Turn(gyroz, target_turn);
	// 若在执行转向，偏航回正补偿为0
	if (left == 1 || right == 1) turn_out = 0;
	pwm_out = vertical_out;

	moto1 = pwm_out - turn_out - turn_plus;
	moto2 = pwm_out + turn_out + turn_plus;
	Moto_Limit(&moto1, &moto2);
	// 安全检测
	if (angle_stop_cmd == 1 || force_stop_cmd == 1)
	{

		moto1 = 0;
		moto2 = 0;
	}

	Load(moto1, moto2);
}

// 角度安全检测-需要0.5s执行一次
void Angle_Check(void)
{
	static int last_last_data = 0;
	static int last_data = 0;
	static int current_data = 0;
	// 更新角度值
	last_last_data = last_data;
	last_data = current_data;
	current_data = roll;
	// 判断三者是否超过阈值
	if (current_data > 60)
	{
		if (last_data > 60 && last_last_data > 60)
		{
			angle_stop_cmd = 1;
			Load(0, 0);
		}
	}
	else if (current_data < -60)
	{
		if (last_data < -60 && last_last_data < -60)
		{
			angle_stop_cmd = 1;
		}
	}
	else if (current_data < 30 && current_data > -30 && last_data < 30 && last_data > -30 && last_last_data < 30 && last_last_data > -30)
	{
			angle_stop_cmd = 0;
	}
}

void Motor_Check(void)
{
	int encoder_min = 10;
	int moto_max = 4000;
	static int last_last_Lencoder = 0;
	static int last_Lencoder = 0;
	static int current_Lencoder = 0;
	static int last_last_Rencoder = 0;
	static int last_Rencoder = 0;
	static int current_Rencoder = 0;
	static int last_last_Lmoto = 0;
	static int last_Lmoto = 0;
	static int current_Lmoto = 0;
	static int last_last_Rmoto = 0;
	static int last_Rmoto = 0;
	static int current_Rmoto = 0;
	// 更新编码器、驱动器值
	last_last_Lencoder = last_Lencoder;
	last_Lencoder = current_Lencoder;
	current_Lencoder = Lencoder;

	last_last_Rencoder = last_Rencoder;
	last_Rencoder = current_Rencoder;
	current_Rencoder = Rencoder;

	last_last_Lmoto = last_Lmoto;
	last_Lmoto = current_Lmoto;
	current_Lmoto = moto1;

	last_last_Rmoto = last_Rmoto;
	last_Rmoto = current_Rmoto;
	current_Rmoto = moto2;
	// 判断电机输出是否超过阈值且实际转速非常小，持续1s
	if (pid_abs(current_Lencoder) < encoder_min && pid_abs(last_Lencoder) < encoder_min && pid_abs(last_last_Lencoder) < encoder_min && pid_abs(current_Lmoto) > moto_max && pid_abs(last_Lmoto) > moto_max && pid_abs(last_last_Lmoto) > moto_max)
	{
		force_stop_cmd = 1;
		Load(0, 0);
	}
	else if (pid_abs(current_Rencoder) < encoder_min && pid_abs(last_Rencoder) < encoder_min && pid_abs(last_last_Rencoder) < encoder_min && pid_abs(current_Rmoto) > moto_max && pid_abs(last_Rmoto) > moto_max && pid_abs(last_last_Rmoto) > moto_max)
	{
		force_stop_cmd = 1;
		Load(0, 0);
	}
}

int pid_abs(int num)
{
	if (num < 0) return -num;
	else return num;
}

void Get_Angle_Mid(void)
{
	med_angle = roll;
}

















